#include <U8g2lib.h>

//U8G2_ST7565_NHD_C12864_1_4W_SW_SPI u8g2(U8G2_R0, 11, 8, 13, 12, U8X8_PIN_NONE);
//U8G2_ST7565_NHD_C12864_1_4W_SW_SPI u8g2(U8G2_R0, 13, 11, 10, 9, U8X8_PIN_NONE);
U8G2_ST7565_ERC12864_1_4W_SW_SPI u8g2 ( U8G2_R0, /* scl=*/  13 , /* si=*/  11 , /* cs=*/  10 , /* rs=*/  9 , /* rse=*/  8 ) ;
int dystans;
char dystans_str[4];
char* rp;
int d1;
int a, b ;
byte outPin = 2;                      // output 800 Hz
byte inPin1 = 6;                      // sw-1 calibration
byte inPin2 = 7;                      // sw-2 measurement

int roznica; 
long srednia;
long sredniaP;
const byte dt = 10;                   //amount of data for the average "idle speed" difference
const byte dt_P = 10;                  //amount of data for the average measurement difference
int sred_wi;

void setup() {
  u8g2.begin(); 
  u8g2.setContrast(40);
  Serial.begin(9600);
  pinMode(inPin1, INPUT_PULLUP);      //setting the pin as a digital input
  pinMode(inPin2, INPUT_PULLUP);  
  //pinMode(13, OUTPUT);         
 // digitalWrite (13, LOW);             //wlaczenie podswietlenia LCD
//  pinMode(8, OUTPUT);               
//  digitalWrite (8, LOW);              //providing ground to power the LCD
  tone(outPin, 800);                  //feeding to the square wave divider
}

void loop() {
   Dipsw();
   sprintf(dystans_str,"%d", abs(d1));
   Display();

}

void Display(){
  if (sred_wi == 0) {rp = "   READY";}
  u8g2.firstPage();
  do {

  u8g2.drawFrame(0,0,128,64);  
  u8g2.drawRFrame(2,2,124,60,3);
    
     u8g2.setFont(u8g2_font_helvB08_tr);
     u8g2.drawStr(30,15,rp); 
     u8g2.setFont(u8g2_font_logisoso24_tn);
     u8g2.drawStr(30,50,dystans_str);
     u8g2.setFont(u8g2_font_helvB12_tf);
     //u8g2.drawStr(85,49,","); 
     //u8g2.drawStr(85,50,"um"); 
     if (d1 < 0) {u8g2.drawStr(85,50,"Fe");}
     if (d1 > 0) {u8g2.drawStr(85,50,"Al");}
  } while ( u8g2.nextPage() ); 
  delay (200);
}

void Oblicz_Sr() {
   srednia = srednia * dt;
   srednia = srednia + dystans;
   srednia = srednia / ( dt + 1);
   sred_wi = srednia;
}

void Oblicz_sr_P() {
   sredniaP = sredniaP * dt_P;
   sredniaP = sredniaP + dystans;
   sredniaP = sredniaP / ( dt_P + 1);
}

void Kalibracja() {
   rp = "CALIBRATION";
   for (int a = 1; a <1000; a++) {
     dystans = 2 * analogRead (1); 
     Oblicz_Sr();
   }
   //d1 = sred_wi;
   d1 = 0;
}

void Pomiar() {
   rp = "MEASURMENT";
   for (int a = 1; a < 1000; a++) {
     dystans = 2 * analogRead (1); 
     Oblicz_sr_P();
   } 
   //d1 = sredniaP;
   d1 = sredniaP - sred_wi; 
}

void Dipsw(){
  byte readPin1, readPin1A, readPin2, readPin2A;
  readPin1 = digitalRead(inPin1);           //reading the status of the SET button
  readPin2 = digitalRead(inPin2);           //read the status of the + button
  delay(1);
  readPin1A = digitalRead(inPin1);
  readPin2A = digitalRead(inPin2);
  if (readPin1 == LOW and readPin1 == readPin1A) {
    Kalibracja();
    delay(100);
  }  
  if (readPin2 == LOW and readPin2 == readPin2A) {
    Pomiar();
    delay(100);
  }
}